#ifndef RIGIDBODY_H_
#define RIGIDBODY_H_

#include "common.h"

#include "../include/Body.h"
#include "shapes.h"
#include "BodyShape.h"
#include "icollision.h"
#include <stack>
#include <list>
#include <string>
#include <boost/shared_ptr.hpp>

namespace physicsworks {

using math::vec3;
using math::mat3;
using math::mat4;

struct DynamicState {
	DynamicState(const vec3& pos, const mat3& ori, const mat4& trans, const vec3& lm, const vec3& am)
	 : position(pos), orientation(ori), transformation(trans), linearMomentum(lm), angularMomentum(am) {}
	DynamicState() : position(vec3::ZERO), orientation(vec3::X, vec3::Y, vec3::Z), linearMomentum(vec3::ZERO),
	                 angularMomentum(vec3::ZERO) { transformation.genTransformation(position, orientation); }
	
	vec3 position;
	mat3 orientation;
	mat4 transformation;
	vec3 linearMomentum;
	vec3 angularMomentum;
};

class RigidBody : public Body {
private:
	RigidBody() : name("anonymous"), gravityCenter(vec3::ZERO) { dynamicStates.push(new DynamicState); }
	
	std::string name;
	std::stack<DynamicState*> dynamicStates;
	std::list<Collision*> collisions;
	BodyState state;
	vec3 gravityCenter;
	real mass;
	mat3 inertiaTensor;
	real restitution;
	real dynamicFriction;
	vec3 force;
	vec3 torque;
	boost::shared_ptr<BodyShape> bodyShape;
	
public:
	RigidBody(BodyShape* bodyShape);
	virtual ~RigidBody();
	void integrate(real timeStep) {}
	void popDynamicState() { delete dynamicStates.top(); dynamicStates.pop(); }
	void removePastDynamicStates();
	void removeCollisions();
	AABox getBoundingBox() const { return bodyShape->getBoundingBox(getPositionVec3(), getOrientationMat3()); }
	void clearForceTorque() { force.zero(); torque.zero(); }
	
	int getId() { return 0; }
	void setName(const char* name) { this->name = name; }
	const char* getName() const { return name.c_str(); }
	const std::string& getNameString() const { return name; }
	void setMass(real m) { mass = m ;}
	real getMass() const { return mass; }
	void setGravityCenter(real cX, real cY, real cZ) { gravityCenter(cX, cY, cZ); }
	real* getGravityCenter() const { return gravityCenter.toReal(); }
	const vec3& getGravityCenterVec3() const { return gravityCenter; }
	void setInertiaTensor(const real* t) { inertiaTensor = t; }
	real* getInertiaTensor() const { return inertiaTensor.toReal(); }
	const mat3& getInertiaTensorMat3() const { return inertiaTensor; }
	void setState(BodyState s) { state = s; }
	BodyState getState() const { return state; }
	void setPosition(const vec3& v) { dynamicStates.top()->position = v; }
	void setPosition(real pX, real pY, real pZ) { dynamicStates.top()->position(pX, pY, pZ); }
	real* getPosition() const { return dynamicStates.top()->position.toReal(); }
	vec3 getPositionVec3() const { return dynamicStates.top()->position; }
	void setOrientation(const real* o) { dynamicStates.top()->orientation = o; }
	void setOrientation(const mat3& o) { dynamicStates.top()->orientation = o; }
	real* getOrientation() const { return dynamicStates.top()->orientation.toReal(); }
	mat3 getOrientationMat3() const { return dynamicStates.top()->orientation; }
	void setRestitution(real r) { restitution = r; }
	real getRestitution() const { return restitution; }
	void setDynamicFriction(real df) { dynamicFriction = df; }
	real getDynamicFriction() const { return dynamicFriction; }
	void addForce(real fX, real fY, real fZ, real pX, real pY, real pZ) {}
	void addForce(real fX, real fY, real fZ) {
		force.x += fX;
		force.y += fY;
		force.z += fZ;
	}
	Body* clone() const { return 0; }
};

}

#endif /*RIGIDBODY_H_*/
